/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: SRF02 I2C Distance Sensor
 *         $Id: dist_i2c.c 18 2014-04-11 05:29:10Z kgills@gmail.com $
 *******************************************************************************
 */

#include <string.h>
#include "maxq_config.h"
#include "i2c/maxq_i2c.h"
#include "comm/maxq_comm.h"
#include "dist.h"
#include "car.h"

/***** Definitions *****/
#define SPEED_OF_SOUND  340290

// Distance sensor register offsets
#define REG_SW_REV      0
#define REG_CMD         0
#define REG_DIST_LOW    2
#define REG_DIST_HIGH   1

// Distance sensor commands
#define CMD_RANGE_IN    0x50
#define CMD_RANGE_CM    0x51
#define CMD_RANGE_US    0x52
#define CMD_FAKE_CM     0x57

#define MAX_RANGE_CM    800
#define MIN_RANGE_CM    10

#define DIST_MODE_ALL   3

/***** Global Data *****/
int dist_mode;

#define BUFFER_SIZE   4
int16_t dist_buffer[NUM_SENSORS][BUFFER_SIZE];
int16_t distance[NUM_SENSORS];

#if (DIST_LOG_SIZE > 0)
int16_t dist_log[DIST_LOG_SIZE][NUM_OBJ_SENSORS];
int16_t dist_msg_log[DIST_LOG_SIZE][3];
int dist_logi;
#endif


/***** File Scope Data *****/
static int curr_sensor;

/****************************************************************************/
int dist_init(void)
{
  int i;
  uint8_t msg;

  // initialize 
  i2c_master_init(I2C_STD_MODE);
//  i2c_enable(); // this must be skipped if SW I2C is used

#if 0
  if (dist_set_id(SENSOR_ADDR(4), SENSOR_ADDR(LEFT)))
    maxq_printf(C"dist_set_id() success\r\n");
  else
    maxq_printf(C"dist_set_id() failure\r\n");
#endif

  // Initialize the current sensor pointer
  curr_sensor = 0;

  for (i = 0; i < NUM_SENSORS; i++) {
    distance[i] = -1;
    // read the SW version
    if (!i2c_master_read(SENSOR_ADDR(i), 1, &msg))
      return 0;
  }

#if (DIST_LOG_SIZE > 0)
  memset(dist_log, 0, sizeof(dist_log));
  dist_logi = 0;
#endif

  dist_mode = DIST_MODE_ALL;

  return 1;
}

/****************************************************************************/
int dist_task(void)
{
  uint8_t msg[3];
  unsigned int temp16;
  int retval = 0;
  int16_t newdist = -1;
  unsigned int sum = 0;
  int cnt, i;

  // Read the previous distance result
  if (!i2c_master_read(SENSOR_ADDR(curr_sensor), 3, msg)) {
    newdist = -1;
    retval = -1;
  }
  else {
    // The sensor will not respond until the measurement is complete
    if (msg[REG_SW_REV] == 0xFF) {
      retval = 0;
    }
    else if (msg[REG_SW_REV] != 0x05) {
      newdist = -1;
      retval = -1;
    }
    else {
      // Update the sensor distance
      temp16 = msg[REG_DIST_HIGH];
      temp16 <<= 8;
      temp16 += msg[REG_DIST_LOW];

      if ( (temp16 < MIN_RANGE_CM) || (temp16 > MAX_RANGE_CM) ) {
        newdist = -1;
        retval = 0;
      } else {
        newdist = temp16;
        retval = 1;
      }
    }
  }

  // Update the buffer
  dist_buffer[curr_sensor][3] = dist_buffer[curr_sensor][2];
  dist_buffer[curr_sensor][2] = dist_buffer[curr_sensor][1];
  dist_buffer[curr_sensor][1] = dist_buffer[curr_sensor][0];
  dist_buffer[curr_sensor][0] = newdist;

  sum = 0;
  cnt = 0;
  for (i = 0; i < BUFFER_SIZE; i++) {
    if (dist_buffer[curr_sensor][i] >= 0) {
      sum += dist_buffer[curr_sensor][i];
      cnt++;
    }

//    if (cnt == 2)
    if (cnt == 1)
      break;
  }

  if (cnt == 2)
    distance[curr_sensor] = sum >> 1;
  else if (cnt == 1)
    distance[curr_sensor] = sum;
  else
    distance[curr_sensor] = -1;

#if (DIST_LOG_SIZE > 0)
  if (curr_sensor == CENTER) {
    dist_log[dist_logi][LEFT] = distance[LEFT];
    dist_log[dist_logi][RIGHT] = distance[RIGHT];
    dist_log[dist_logi][CENTER] = distance[CENTER];
//dist_msg_log[dist_logi][0] = msg[1];
//dist_msg_log[dist_logi][1] = msg[2];
    if (++dist_logi == DIST_LOG_SIZE)
      dist_logi = 0;
  }
else if (curr_sensor == RIGHT) {
  dist_msg_log[dist_logi][0] = msg[0];
  dist_msg_log[dist_logi][1] = msg[1];
  dist_msg_log[dist_logi][2] = msg[2];
}
#endif

  // Update the current sensor pointer
  curr_sensor++;
#if 0
  if (curr_sensor >= NUM_SENSORS)
    curr_sensor = 0;
#else
  if (curr_sensor > RIGHT)
    curr_sensor = LEFT;
#endif

  // Initiate the next distance measurement
  msg[0] = 0;     // command register is at address 0
  if ( (dist_mode == DIST_MODE_ALL) || (dist_mode == curr_sensor) )
    msg[1] = CMD_RANGE_CM;  // initiate measurement
  else
    msg[1] = CMD_FAKE_CM;
  i2c_master_write(SENSOR_ADDR(curr_sensor), 2, msg);

  return retval;
}

/****************************************************************************/
int dist_set_id(int curr_id, int new_id)
{
  uint8_t msg[2];

  if (curr_id < 0)
    curr_id = SENSOR_ADDR_BASE;

  msg[0] = 0;     // command register is at address 0

  msg[1] = 0xA0;
  if (!i2c_master_write(curr_id, 2, msg))
    return 0;

  msg[1] = 0xAA;
  if (!i2c_master_write(curr_id, 2, msg))
    return 0;

  msg[1] = 0xA5;
  if (!i2c_master_write(curr_id, 2, msg))
    return 0;

  msg[1] = new_id << 1;
  if (!i2c_master_write(curr_id, 2, msg))
    return 0;

  return 1;
}

/****************************************************************************/
#if (DIST_LOG_SIZE > 0)
void dist_log_display(void)
{
  int i;
  for (i = dist_logi; i < DIST_LOG_SIZE; i++) {
//    maxq_printf(C"%6d %6d %6d\r\n", dist_log[i][CENTER], dist_log[i][LEFT], dist_log[i][RIGHT]);
maxq_printf(C"%6d %6d %6d", dist_log[i][CENTER], dist_log[i][LEFT], dist_log[i][RIGHT]);
maxq_printf(C"   %02x %02x %02x\r\n", dist_msg_log[i][0], dist_msg_log[i][1], dist_msg_log[i][2]);
  }
  for (i = 0; i < dist_logi; i++) {
//    maxq_printf(C"%6d %6d %6d\r\n", dist_log[i][CENTER], dist_log[i][LEFT], dist_log[i][RIGHT]);
maxq_printf(C"%6d %6d %6d", dist_log[i][CENTER], dist_log[i][LEFT], dist_log[i][RIGHT]);
maxq_printf(C"   %02x %02x %02x\r\n", dist_msg_log[i][0], dist_msg_log[i][1], dist_msg_log[i][2]);
  }
}
#endif
